/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 *
 * $Id: SpeedCtrl_Dummy.cpp,v 1.11 2009-05-13 07:18:23 amaula Exp $
 *
 * \file
 * \brief Dummy implementation for SpeedCtrl Server interface.
 *
 * This implementation can act as an example / codebase for any
 * SpeedCtrl server based module implementation. This code follows a
 * pseudogood coding style, and also provides a basic parameter
 * parser that can be directly used with slight modifications.
 * 
 * \todo 
 */
#include <stdio.h>
#include <stdlib.h>
#include "owndebug.h"
#include "SpeedCtrlServer.hpp"
#include "binbag.h"

//*****************************************************************************
#define GIMI_CLIENT_API (20000)
#include "gimi.h"
//*****************************************************************************
static int verbose = 1;
static MaCI::SpeedCtrl::CSpeedCtrlServer *speedCtrlServer; 
//*****************************************************************************
using namespace MaCI::SpeedCtrl;
//*****************************************************************************

void showUsage()
{
  fprintf(stderr,"\n");
  fprintf(stderr,"Usage:\tSpeedCtrl_Dummy [OPTIONS]\n");
  fprintf(stderr,"  [-u addr]\t\t\tUse 'addr' as GIMnet AccessPoints address\n");
  fprintf(stderr,"  [-p port]\t\t\tUse 'port' as GIMnet AccessPoints port\n");
  fprintf(stderr,"  [-n name]\t\t\tUse 'name' as this nodes GIMnet name\n");
  fprintf(stderr,"  [-G name]\t\t\tUse 'name' as MaCIGroup for this server\n");
  fprintf(stderr,"  [-I name]\t\t\tUse 'name' as Instance for this server\n");
  fprintf(stderr,"  [-K key]\t\t\tUse 'key' as access key for SpeedCtrl interface\n");
  
  fprintf(stderr,"  [-v]\t\t\t\tIncrease verbosity by 1\n");
  fprintf(stderr,"  [-q]\t\t\t\tQuiet mode\n");
  fprintf(stderr, "\n\n");
}
//*****************************************************************************
static void SendSpeedEvent(const TSpeed *aSpeed)
{
 
  CSpeedCtrlData speed;
  gim::binbag::CBinBag bag;
  MaCI::Common::TTimestamp tstamp;
  tstamp.SetToCurrentTime();
  speed.EncodeTo(&bag); 
  speed.Reset();
  speed.SetCommand(TCommand(KEventSpeed));
  speed.SetSpeed(*aSpeed);
  speed.SetTimestamp(tstamp);

  if(speedCtrlServer->SendSpeedEvent(speed)){
    dPrint(1,"SENT BACK EVENT (%f, %f, %f",
           aSpeed->speed,
           aSpeed->angularSpeed,
           aSpeed->acceleration);
    

  }else{
    dPrint(1,"Failed to send event");
  }
}
//*****************************************************************************

static void CommandSetSpeed(const TSpeed *aSpeed)
{
  // Got SetSpeed()
  dPrint(1,"COMMAND: SET_SPEED (%f, %f, %f)",
         aSpeed->speed,
         aSpeed->angularSpeed,
         aSpeed->acceleration);

  //Then send back an event
  
  SendSpeedEvent(aSpeed);
}
//*****************************************************************************

static void CommandSetStop()
{
  // Got SetStop()
  dPrint(1,"COMMAND: SET_STOP ()");
  //Then send back an event
  const TSpeed speed(0,0,0.2);
  SendSpeedEvent(&speed);
}
//*****************************************************************************

static void ProcessCommand(const CSpeedCtrlData &aCommand)
{
  // YES! Take the Command element pointer and check command type.
  const TCommand *cmd = aCommand.GetCommand();
  assert(cmd != NULL);
  
  // Switch based on command type
  switch(cmd->cmd) {
    
  case KCommandSetStop: {
    // Got command SetStop
    CommandSetStop();
    break;
  }
    
  case KCommandSetSpeed: {
    // Got speed command?
    const TSpeed *speed = aCommand.GetSpeed();
    if (speed) {
      // Got command SetSpeed()
      CommandSetSpeed(speed);
      
    } else {
      dPrint(1,"WARNING: No speed element available even SetSpeed was commanded!");
      
    }
    break;
  }
  case KCommandTimeout:{
    dPrint(1,"ALERT! No command within timeout. Calling SetStop!");
    CommandSetStop();
    break;
  }
  default:
    dPrint(1,"No implementation for command %d!", cmd->cmd);
    break;
  }

}
//*****************************************************************************

int main(int argc, char *argv[]) 
{
  // GIMnet parameters
  std::string gimnetAP = "asrobo.hut.fi";
  int gimnetAPPort = 50002;
  std::string gimnetName = "";
  std::string macigroup;
  std::string interfaceInstance = "Dummy";

  // Objects
  using namespace MaCI;
  bool run = true;
  gimi::GIMI g;
  MaCICtrl::CMaCICtrlServer msc(&g);
  SpeedCtrl::CSpeedCtrlServer scs(&msc, 0, "Very Very Dummy."); 
  speedCtrlServer = &scs;
  unsigned char akey[16];
  
  // Zero key
  memset(akey,0x00,sizeof(akey));

  // Init Debug lib
  debugInit();
  debugSetGlobalDebugLvl(4);
  debugSetLogFilename("SpeedCtrl_Dummy.log");

  // Greet
  fprintf(stderr,
	  "\nSpeedCtrl_Dummy - MaCI SpeedCtrl Interface implementation for Dummy - v0.0\n");

  // Parse parameters
  bool parse = true;
  while(parse) {
    int c;
    c = getopt(argc, argv, "I:n:u:p:G:K:hqv");
    switch(c) {

      /////////////////////////////////
      ////// Application specific /////
      /////////////////////////////////
    case 'K':
      strncpy((char*)akey, optarg, sizeof(akey));
      dPrint(3,"Using MaCI Key: '%s'", optarg);
      break;

      /////////////////////////////////
      ///// GIMnet parameters /////////
      /////////////////////////////////
    case 'n':
      gimnetName = optarg;
      dPrint(3,"Using GIMnet Name '%s'", optarg);
      break;

    case 'u':
      gimnetAP = optarg;
      dPrint(3,"Using GIMnetAP address '%s'", optarg);
      break;

    case 'p':
      gimnetAPPort = atoi(optarg);
      dPrint(3,"Using GIMnetAP port '%d'", gimnetAPPort);
      break;

    case 'G':
      macigroup = optarg;
      dPrint(3,"Using MaCIGroup '%s'", optarg);
      break;
    case 'I':
      interfaceInstance = optarg;
      dPrint(3,"Using Interface Instance '%s'", optarg);
      break;
      //////////////////////////////////
      //// GENERIC:                 ////
      //////////////////////////////////
    case 'v':
      // Increase only if not previously set to 0 (by 'quiet')
      if (verbose > 0)
	debugSetGlobalDebugLvl(++verbose);
      break;
      
    case 'q':
      verbose = 0;
      debugSetGlobalDebugLvl(0);
      break;
      
    case 'h':
    case '?':
      showUsage();
      exit(1);
      break;
      
    case -1:
      parse = false;
      break;
    }
  }
  
  // Set MaCI parameters - Group for MaCICtrl
  msc.SetGroupName(macigroup);
  
  // Interface instance
  scs.SetInterfaceInstanceName(interfaceInstance);

  //Set speed Limits
  scs.SetSpeedLimits(0.5,1.8,0.2,-0.5,-1.8, 0.0);

  //Set command timeout time to 300 ms
  scs.SetCommandTimeoutTime(200);



  // - Key for SpeedCtrlServer
  if (akey[0]) {
    dPrint(1,"Using the provided key '%s' as Authentication key", akey);
    scs.SetServerInterfaceAuthentication(MaCICtrl::TAuthentication(akey, sizeof(akey)), // Provided key
                                         1, // One simultaneous access only
                                         10000); // Timeout in 10 seconds.
  }
  
  int r;
  if ( (r = g.connectToHubEx(gimnetAP, gimnetAPPort, gimnetName) ) != GIMI_OK) {
    dPrint(1,"Failed to connect to GIMnet AccessPoint '%s:%d' with name '%s': '%s'",
	   gimnetAP.c_str(), 
	   gimnetAPPort, 
	   gimnetName.size()?gimnetName.c_str():"<anonymous>",
	   gimi::getGimiErrorString(r).c_str());
    
  } else {
    dPrint(2,"Succesfully connected to GIMnet at AccessPoint '%s:%d' with name '%s': '%s'",
	   gimnetAP.c_str(), 
	   gimnetAPPort, 
	   gimnetName.size()?gimnetName.c_str():"<anonymous>",
	   gimi::getGimiErrorString(r).c_str());
    
    MaCI::EMaCIError e;
    if ( (e = scs.Open()) != MaCI::KMaCIOK) {
      dPrint(1,"Failed to Open SpeedCtrlServer instance! (%s)",
	     GetErrorStr(e).c_str());
      
    } else {
      ////// Execute test section ///////

      const int interval = 0;

      // Run as long as 'run' is set.
      while (run) {
        using namespace MaCI::SpeedCtrl;

        // Construct Data element
        CSpeedCtrlData cdata;

        // Now, sit back and wait for command to arrive (for 1000ms)
        // set SkipFlag to 'true', so the received commands are not buffered up.
        e = scs.WaitCommand(cdata, 1000, true);

        // Got command? (No timeout or error)
        if (e == MaCI::KMaCIOK) {
          // Yes, got command, pass it to another function for parsing.
          ProcessCommand(cdata);

          // Sleep
          ownSleep_ms(interval);

        } else if (e == MaCI::KMaCITimeout) {
          // No, timeout waiting for command. Print and wait again.
          dPrint(8,"Timeout waiting for Command");
                 
        } else {
          // Quck? Some error occured? Print it, sleep an extra second and try again.
          dPrint(1,"Failed to WaitCommand()! (%s)",
                 GetErrorStr(e).c_str());
          ownSleep_ms(1000);
        }
      }
    }
  }

  return 0;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************


